#include "Quaternion.h"
#include <algorithm>
#include "Vector3.h"
#include <cmath>

namespace soft_renderer
{
	Quaternion::Quaternion(double x, double y, double z, double w) : _x(x), _y(y), _z(z), _w(w)
	{
	}

	double Quaternion::x()
	{
		return _x;
	}

	double Quaternion::y()
	{
		return _y;
	}

	double Quaternion::z()
	{
		return _z;
	}

	double Quaternion::w()
	{
		return _w;
	}

	void Quaternion::set(double x, double y, double z, double w)
	{
		_x = x;
		_y = y;
		_z = z;
		_w = w;
	}

	void Quaternion::setFromAxisAngle(Vector3 axis, double angle)
	{
		double halfAngle = angle / 2;
		double s = std::sin(halfAngle);
		_x = axis.x() * s;
		_y = axis.y() * s;
		_z = axis.z() * s;
		_w = std::cos(halfAngle);
	}

	void Quaternion::identity()
	{
		set(0, 0, 0, 1);
	}

	void Quaternion::multiplyQuaternions(const Quaternion& a, const Quaternion& b)
	{
		// from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
		const double qax = a._x, qay = a._y, qaz = a._z, qaw = a._w;
		const double qbx = b._x, qby = b._y, qbz = b._z, qbw = b._w;
		_x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
		_y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
		_z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
		_w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;
	}
}
